#include "hb_msgs/msg/heart_beat.hpp"
#include "modbus.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64.hpp"

class SenkyLaser_node : public rclcpp::Node
{
  private:
	// usart tty device file path
	std::string tty_path = "/dev/ttyUSB0";
	// publish period in ms
	int frequency_level = 4;
	// distance result offset, NOTE: this offset should already written into device register, so
	// the distance result we read from register is already real value(real = measure - offset)
	double distance_offset = 0;
	// address of the device
	int device_address = 0;

	// sample once when this timer expires
	std::shared_ptr<rclcpp::TimerBase> sample_timer;
	// modbus communication controller class
	std::shared_ptr<Modbus_controller> controller;
	// used to publish sample result(in mm, valid when >0)
	rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr publisher;
	// this smart pointer keeps on_set_parameter_callback valid
	rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr setparamcall;

	int fd;

	rclcpp::TimerBase::SharedPtr hb_timer;
	rclcpp::Publisher<hb_msgs::msg::HeartBeat>::SharedPtr hb_pub;
	int error_code;

	// if this host is bypassed
	bool bypassed = false;
	std::thread listen_thread;

	/**
	 * @brief enable or disable bypass mode. in bypass mode, we release rs485 bus
	 *
	 * @param enable true: enable, false: disable
	 * @return int 0: success, <0: failed
	 */
	int enable_bypass(bool enable)
	{
		this->bypassed = enable;
		return 0;
	}

	/**
	 * @brief config parameters for the device
	 *
	 * @param address device address to write into, from 1 to 247, 0 for broadcast
	 * @param offset distance offset value, from -2000.0mm to 2000.0mm
	 * @param frequency sample frequency level, 0: single, 1: 5Hz, 2: 10Hz, 3: 20Hz, 4: 30Hz
	 * @return int -1: bad args, -2: failed, 0: success
	 */
	int config_device(unsigned int address, double offset, int frequency)
	{
		int err = 0;
		auto rate = rclcpp::Rate(std::chrono::milliseconds(1000));
		// if bypassed, should not config
		if (this->bypassed)
		{
			RCLCPP_INFO(this->get_logger(), "host bypassed, skip config");
			return 0;
		}

		if (this->controller->file_lock->trylock_for(20000000) < 0)
		{
			RCLCPP_ERROR(this->get_logger(), "failed to get tty file mutex");
			return -2;
		}
		do
		{
			// argument check
			if (address < 0 || address > 247)
			{
				RCLCPP_ERROR(this->get_logger(),
							 "bad config argument: address should be in 0 ~ 247");
				err = -1;
				break;
			}
			if (offset < -2000 || offset > 2000)
			{
				RCLCPP_ERROR(this->get_logger(),
							 "bad config argument: offset should be in -2000 ~ 2000");
				err = -1;
				break;
			}
			if (level_to_Hz(frequency) < 0)
			{
				RCLCPP_ERROR(this->get_logger(), "bad config argument: frequency level should "
												 "be choosed from 1, 2, 3 and 4");
				err = -1;
				break;
			}
		} while (0);
		if (err == -1)
		{
			this->controller->file_lock->unlock();
			return -1;
		}

		do
		{
			// read firmware version
			err = this->controller->read_version();
			if (err < 0)
			{
				RCLCPP_WARN(this->get_logger(),
							"failed to read firmware version(%d), moving on", err);
			}
			else
			{
				RCLCPP_INFO(this->get_logger(), "device firmware version: 0x%04x", err);
			}
			long id = this->controller->read_id();
			if (id < 0)
			{
				RCLCPP_WARN(this->get_logger(), "failed to read product id(%ld), moving on",
							id);
			}
			else
			{
				RCLCPP_INFO(this->get_logger(), "read product id: %08x", (unsigned int)id);
			}
			err = this->controller->read_address();
			if (err < 0)
			{
				RCLCPP_WARN(this->get_logger(), "failed to read device address(%d), moving on",
							err);
			}
			else
			{
				RCLCPP_INFO(this->get_logger(), "read device address: %d", err);
			}

			int retry;
			for (retry = 0; retry < 5; retry++)
			{
				// set sample frequency
				err = this->controller->write_frequency(frequency);
				if (err < 0)
				{
					RCLCPP_ERROR(this->get_logger(), "failed to write sample frequency(%d)",
								 err);
					continue;
				}
				RCLCPP_INFO(this->get_logger(), "sample frequency set to %d Hz",
							level_to_Hz(frequency));
				// wait till ready
				rate.sleep();
				// write offset
				err = this->controller->write_offset(offset * 10);
				if (err < 0)
				{
					RCLCPP_ERROR(this->get_logger(), "failed to write distance offset(%d)",
								 err);
					continue;
				}
				RCLCPP_INFO(this->get_logger(), "device distance offset set to %d.%d mm",
							(int)offset, ((int)(offset * 10)) % 10);
				rate.sleep();
				break;
			}
			if (retry >= 5)
			{
				break;
			}

			// read device tempreature
			err = this->controller->read_tempreature();
			if (err == 0x7FFFFFFF)
			{
				RCLCPP_WARN(this->get_logger(), "failed to read device tempreature, moving on");
			}
			else if (err < -100 || err > 600)
			{
				RCLCPP_WARN(this->get_logger(),
							"abnormal device tempreature(%f centigrade), sensors may not "
							"working well under this condition",
							err / 10.0);
			}
			else
			{
				RCLCPP_INFO(this->get_logger(), "read device tempreature: %f centigrade",
							err / 10.0);
			}

			RCLCPP_INFO(this->get_logger(), "device config done");
			this->controller->file_lock->unlock();
			return 0;
		} while (0);
		RCLCPP_INFO(this->get_logger(), "failed to config");
		this->controller->file_lock->unlock();
		return -2;
	}

	// called when timer expires
	void sample_callback(void)
	{
		if (this->bypassed)
		{
			RCLCPP_DEBUG(this->get_logger(), "host bypassed, skip sample");
			this->error_code = 0;
			return;
		}

		// ros2 run senky_laser laser_node --ros-args --log-level debug
		RCLCPP_DEBUG(this->get_logger(), "enter sample timer callback");
#ifdef TEST_TTY
		this->controller->tty_test();
		return;
#else
		long int result;
		std_msgs::msg::Float64 msg;
		// read data register
		result = this->controller->read_distance();
		if (result <= -20000)
		{
			switch (result)
			{
				case -20000:
					RCLCPP_WARN(this->get_logger(),
								"distance exceed range or bad target surface");
					this->error_code = -1;
					break;
				case -20001:
					RCLCPP_WARN(this->get_logger(),
								"read distance error: failed to read or write tty");
					this->error_code = -2;
					break;
				case -20002:
					RCLCPP_WARN(this->get_logger(),
								"read distance error: bad recv crc or incorrect length");
					this->error_code = -2;
					break;
				case -20004:
					RCLCPP_WARN(this->get_logger(),
								"read distance error: tty mutex locked and timeout");
					this->error_code = -3;
					break;
				default:
					RCLCPP_WARN(this->get_logger(), "read distance error: unexpected error");
					break;
			}
		}
		else
		{
			this->error_code = 0;
		}
		// write and publish message
		msg.set__data(result > -20000 ? result * 0.1 : result);
		this->publisher->publish(msg);
		RCLCPP_DEBUG(this->get_logger(), "laser distance: %f",
					 result > -20000 ? result * 0.1 : result);
#endif
	}

	void listen_func(void)
	{
		uint8_t frame[16];
		int err;
		bool incoming = false;
		std_msgs::msg::Float64 msg;
		while (rclcpp::ok())
		{
			do
			{
				// sleep and pass if not bypassed
				if (!this->bypassed)
				{
					usleep(10000);
					break;
				}

				err = this->controller->read_raw(frame, 16);
				if (err <= 0)
				{
					incoming = false;
					break;
				}

				if (frame[2] + 5 == err)
				{
					// device -> host
					if (!incoming)
					{
						// not expecting
						break;
					}
					auto dat = uint8_to_int32(frame[3], frame[4], frame[5], frame[6]);
					msg.data = dat > 0 ? dat * 0.1 : -20000;
					this->publisher->publish(msg);
					incoming = false;
				}
				else if (err == 8)
				{
					// host -> device
					if (frame[1] == 0x03 && frame[2] == 0x00 && frame[3] == 0x04)
					{
						incoming = true;
					}
				}
				else
				{
					incoming = false;
				}
			} while (0);
			usleep(100);
		}
		RCLCPP_ERROR(this->get_logger(), "listen thread exit");
	}

	// called when parameters were set by client
	rcl_interfaces::msg::SetParametersResult parameter_callback(
		const std::vector<rclcpp::Parameter> &params)
	{
		rcl_interfaces::msg::SetParametersResult ret;
		ret.successful = true;

		if (this->bypassed)
		{
			for (auto par : params)
			{
				if (par.get_name() == "bypassed")
				{
					this->enable_bypass(par.as_bool());
				}
				else
				{
					ret.successful = false;
					ret.reason = "should not set parameter other than \"bypassed\" when host "
								 "is bypassed";
				}
			}
			return ret;
		}

		// iteration
		for (auto par : params)
		{
			if (par.get_name() == "tty_path")
			{
				ret.successful = false;
				ret.reason = "should not change tty_path on the run";
			}
			else if (par.get_name() == "frequency_level")
			{
				if (level_to_Hz(par.as_int()) <= 0)
				{
					ret.successful = false;
					ret.reason =
						"frequency_level can only be 1(5Hz), 2(10Hz), 3(20Hz) or 4(30Hz)";
				}
				else
				{
					// seems there a chance to fail if we dont go idle first
					this->controller->write_status(0);
					if (this->controller->write_frequency(par.as_int()) < 0)
					{
						ret.successful = false;
						ret.reason = "failed to write frequency register";
						// write back
						this->controller->write_status(2);
					}
					else
					{

						this->frequency_level = par.as_int();
						// previous timer will be auomatically released since there is no
						// reference to it now
						this->sample_timer = this->create_wall_timer(
							std::chrono::milliseconds(1000 /
													  level_to_Hz(this->frequency_level)),
							std::bind(&SenkyLaser_node::sample_callback, this));
					}
					// remember to go back
					this->controller->write_status(2);
				}
			}
			else if (par.get_name() == "distance_offset")
			{
				if (par.as_double() < -2000 || par.as_double() > 2000)
				{
					ret.successful = false;
					ret.reason = "offset must be in the range of -2000 and 2000";
				}
				else if (this->controller->write_status(0),
						 this->controller->write_offset(par.as_double() * 10) < 0)
				{
					ret.successful = false;
					ret.reason = "failed to write offset register";
				}
				else
				{
					this->distance_offset = par.as_double();
				}
				// remember to go back
				this->controller->write_status(2);
			}
			else if (par.get_name() == "device_address")
			{
				if (par.as_int() < 0 || par.as_int() > 247)
				{
					ret.successful = false;
					ret.reason = "device address exceed range(0 ~ 247, 0 for broadcast)";
				}
				else
				{
					auto bak = this->controller->get_address();
					this->controller->set_address(0);
					if (this->controller->write_address(par.as_int()) < 0)
					{
						ret.successful = false;
						ret.reason = "failed to write address register";
						this->controller->set_address(bak);
					}
					else
					{
						if (this->controller->save_params() < 0)
						{
							ret.reason = "address written but failed to write into rom";
						}
						this->controller->set_address(par.as_int());
						this->device_address = par.as_int();
						ret.successful = true;
					}
				}
			}
			else
			{
				ret.successful = false;
				ret.reason = "unsupported or undeclared parameter";
			}
		}
		return ret;
	}

	void hb_callback(void)
	{
		hb_msgs::msg::HeartBeat hb;
		hb.name = this->get_fully_qualified_name();
		hb.stamp = rclcpp::Clock().now();

		switch (this->error_code)
		{
			case -1:
				hb.code = -1;
				hb.reason = "data invalid";
				this->error_code = 0;
				break;
			case -2:
				hb.code = -2;
				hb.reason = "communication error";
				break;
			case -3:
				hb.code = -3;
				hb.reason = "failed to get lock";
			default:
				hb.code = 0;
				hb.reason = "ok";
				break;
		}
		this->hb_pub->publish(hb);
	}

  public:
	int success = 0;
	SenkyLaser_node(std::string name) : Node(name)
	{
		RCLCPP_INFO(this->get_logger(), "laser node started");
		// decleare parameters
		this->declare_parameter<std::string>("tty_path", tty_path);
		this->declare_parameter<int>("frequency_level", frequency_level);
		this->declare_parameter<double>("distance_offset", distance_offset);
		this->declare_parameter<int>("device_address", device_address);
		this->declare_parameter<bool>("bypassed", bypassed);
		this->get_parameter("tty_path", tty_path);
		this->get_parameter("frequency_level", frequency_level);
		this->get_parameter("distance_offset", distance_offset);
		this->get_parameter("device_address", device_address);
		this->get_parameter("bypassed", bypassed);
		RCLCPP_INFO(this->get_logger(),
					"parameters using: tty_path= %s, frequency_level= %d(%d Hz), "
					"distance_offset= %.1fmm, device_address= %d",
					tty_path.c_str(), frequency_level, level_to_Hz(frequency_level),
					distance_offset, device_address);
		if (this->bypassed)
		{
			RCLCPP_INFO(this->get_logger(), "NOTE: this host is bypassed by launch parameter");
		}
		// called when parameters were set by client
		this->setparamcall = this->add_on_set_parameters_callback(
			std::bind(&SenkyLaser_node::parameter_callback, this, std::placeholders::_1));

		// open tty file
		RCLCPP_INFO(this->get_logger(), "trying to open tty file...");
		this->fd = open(tty_path.c_str(), O_RDWR);
		if (this->fd <= 0)
		{
			if (this->fd == -1)
			{
				// need permission to access device file
				RCLCPP_ERROR(this->get_logger(),
							 "need permission to access file %s, run ‘sudo chmod 666 %s’ "
							 "and restart this node",
							 tty_path.c_str(), tty_path.c_str());
			}
			else
			{
				perror("failed to open tty file");
				RCLCPP_ERROR(this->get_logger(),
							 "failed to open tty file %s with error code %d, file may not "
							 "exist, check your "
							 "physical connection "
							 "or change tty path parameter 'tty_path'",
							 tty_path.c_str(), this->fd);
			}
			return;
		}

		// create modbus controller class and move on
		this->controller = std::make_shared<Modbus_controller>(
			this->fd, device_address,
			tty_path.substr(tty_path.find_last_of('/') == std::string::npos
								? 0
								: tty_path.find_last_of('/') + 1) +
				"_mutex");

		RCLCPP_INFO(this->get_logger(), "tty file open");

		if (this->config_device(device_address, distance_offset, frequency_level) < 0)
		{
			return;
		}

		// create a timer
		this->sample_timer = this->create_wall_timer(
			std::chrono::milliseconds(1000 / level_to_Hz(frequency_level)),
			std::bind(&SenkyLaser_node::sample_callback, this));

		this->publisher = this->create_publisher<std_msgs::msg::Float64>("laser_distance", 10);
		// create publisher to publish distance data(in mm)
		RCLCPP_INFO(this->get_logger(), "publishing distance data(std_msgs/msg/Float64) to "
										"topic 'laser_distance' periodically...");

		this->hb_pub = this->create_publisher<hb_msgs::msg::HeartBeat>("/heartbeat", 1);
		this->hb_timer = this->create_wall_timer(
			std::chrono::milliseconds(200), std::bind(&SenkyLaser_node::hb_callback, this));

		this->listen_thread = std::thread(std::bind(&SenkyLaser_node::listen_func, this));
		this->listen_thread.detach();
		success = 1;
	}

	~SenkyLaser_node()
	{
		if (this->fd > 0)
		{
			close(this->fd);
		}
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<SenkyLaser_node>("laser_node");
	if (node->success) rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
